#include <math.h>
#include <stdlib.h>
#include "mt6701.h"

SPI_HandleTypeDef *hspi_mt6701;

// 外部变量定义
extern uint16_t last_raw_angle;
extern int32_t track_count;

typedef struct {
    uint16_t last_raw_angle;  // 上一次的原始角度值
    int32_t track_count;      // 多圈计数
    float last_angle;         // 上一次的角度值
} MT6701_Tracker;

// 角度跟踪器实例
MT6701_Tracker mt6701_tracker = {
    .last_raw_angle = 0,
    .track_count = 0,
    .last_angle = 0.0f
};

// 初始化外部变量
uint16_t last_raw_angle = 0;
int32_t track_count = 0;
uint8_t current_byte_order_mode = 0;

// MT6701接收缓冲区
uint8_t mt6701_rx_data[3] = {0};

// 最新解码的角度值
float encoder_angle = 0.0f;
float motor_logic_angle = 0.0f;
float position_cycle = 2 * 3.1415926535f; // 2π

// CRC计算函数
uint8_t calculate_crc(uint32_t data)
{
    uint8_t crc = 0;
    uint8_t i, j;
    
    for (i = 0; i < 26; i++)  // MT6701数据为26位
    {
        crc ^= ((data >> (25 - i)) & 0x01);
        for (j = 0; j < 7; j++)
        {
            if (crc & 0x40)
            {
                crc = (crc << 1) ^ 0x07;
            }
            else
            {
                crc = (crc << 1);
            }
        }
    }
    
    return crc & 0x3F; // 只返回低6位
}

// 周期差计算函数
float cycle_diff(float diff, float cycle)
{
    while (diff > cycle / 2) diff -= cycle;
    while (diff < -cycle / 2) diff += cycle;
    return diff;
}

// SPI接收完成回调函数在main.c中实现

void MT6701_Init(SPI_HandleTypeDef *hspi)
{
  hspi_mt6701 = hspi;

  // CS引脚已经在main.c的MX_GPIO_Init中初始化，这里不再重复初始化

  // 初始化为高电平（未选中）
  MT6701_CS_DISABLE();
  HAL_Delay(10); // 确保有足够的初始化时间
  
  // 初始化角度跟踪器
  mt6701_tracker.last_raw_angle = 0;
  mt6701_tracker.track_count = 0;
  mt6701_tracker.last_angle = 0.0f;
  
  // 初始化全局变量
  encoder_angle = 0.0f;
  motor_logic_angle = 0.0f;
  position_cycle = 2 * 3.1415926535f; // 2π
  
  // 注意：不在这里启动DMA传输，而是在main函数中手动启动
  // 避免在初始化阶段就进入可能导致问题的回调函数
  MT6701_CS_DISABLE();
}

// 启动MT6701的数据接收
void MT6701_StartReading(void)
{
  HAL_GPIO_WritePin(MT6701_CS_GPIO_Port, MT6701_CS_Pin, GPIO_PIN_RESET);
  if (HAL_SPI_TransmitReceive_DMA(hspi_mt6701, mt6701_rx_data, mt6701_rx_data, 3) != HAL_OK)
  {
    // 启动失败时，将CS引脚设置为高电平
    MT6701_CS_DISABLE();
  }
}

uint16_t MT6701_ReadRawData(void)
{
  // 直接返回最新的原始角度值
  return last_raw_angle;
}

float MT6701_GetAngle(void)
{
  // 返回当前角度值（弧度）
  return encoder_angle * 180.0f / 3.14159265359f; // 转换为度数
}

float MT6701_GetMultiTurnAngle(void)
{
  // 获取当前角度值
  float current_angle = MT6701_GetAngle();
  
  // 计算多圈角度值（使用motor_logic_angle已处理好多圈问题）
  float multi_turn_angle = motor_logic_angle * 180.0f / 3.14159265359f; // 转换为度数
  
  return multi_turn_angle;
}